#include <mex.h>
#include <math.h>
#include "shapeAndResponse.h"



void cpVectorDouble2Float(float * p_dest, double *p_src, int num_elem){
	for( int i = 0;i<num_elem; ++i)
		p_dest[i]=(float)p_src[i];
}


void *getPtrToField(const mxArray *p_struct, int i, const char* field_name){
    const mxArray *p_field=mxGetField(p_struct,i,field_name);
    return (void*)mxGetPr(p_field);
}

void mexFunction( int nlhs, mxArray *plhs[],
                  int nrhs, const mxArray *prhs[] ){
	// access the variables
	double dims[6];
	int i=0;
	dims[2] = floor(mxGetScalar(prhs[i++])+0.5);// n_image
	dims[3] = floor(mxGetScalar(prhs[i++])+0.5);// n_orient
	dims[5] = floor(mxGetScalar(prhs[i++])+0.5);// n_length
	
	int h =(int) floor(mxGetScalar(prhs[i++])+0.5f);
	// image sizes and zoom ratios
	double *p_sx = mxGetPr(prhs[i++]);
	double *p_sy = mxGetPr(prhs[i++]);
	
	double *p_zx = mxGetPr(prhs[i++]);
	double *p_zy = mxGetPr(prhs[i++]);
	
	// get M2 maps
	float **p_M2 = (float**)mxCalloc((int)(dims[2]*dims[3]*dims[5]),sizeof(float*));
	for ( int i_page = 0; i_page<dims[2]*dims[3]*dims[5]; ++i_page){
		mxArray *p_page = mxGetCell(prhs[i],i_page);
		p_M2[i_page]=(float*)mxGetPr(p_page);
	}
	i++;
		
	// get root tan, tan and plane structure
	const mxArray *p_root_tan = prhs[i++];
	if( !mxIsStruct(p_root_tan)){
		mexPrintf("Error, not a structure, p_root_tan.\n");
	}
   
	const mxArray *p_tan=prhs[i++];
	const mxArray *p_plane = mxGetField(p_tan,0,"plane");
	if( !mxIsStruct(p_plane)){
		mexPrintf("Error, not a structure, p_plane.\n");
	}
	

	// plane parameters
	plane target_plane;
	double *p_tmp_array=(double*)getPtrToField(p_plane,0,"nt");
	cpVectorDouble2Float(&target_plane.nt[0],p_tmp_array,4);
	p_tmp_array = (double*)getPtrToField(p_plane, 0, "origin");
	cpVectorDouble2Float(&target_plane.origin[0],p_tmp_array,3);
	p_tmp_array = (double*)getPtrToField(p_plane, 0, "u");
	cpVectorDouble2Float(&target_plane.bu[0],p_tmp_array,3);
	p_tmp_array = (double*)getPtrToField(p_plane,0,"v");
	cpVectorDouble2Float(&target_plane.bv[0],p_tmp_array,3);
	target_plane.width = *(double*)getPtrToField(p_plane,0,"w");
	target_plane.height = *(double*)getPtrToField(p_plane,0,"h");
	target_plane.stp =*(double*)getPtrToField(p_plane,0,"stp");
	

	target_plane.height*=target_plane.stp; // plane size in plane coordinate system
	target_plane.width *= target_plane.stp;
	dims[0]=target_plane.height;
	dims[1]=target_plane.width;


	// root tan parameters
	double *p_root_tan_rotation = (double*)getPtrToField(p_root_tan, 0, "rotationMat");
	float root_tan_rotation[9];
	cpVectorDouble2Float(root_tan_rotation,p_root_tan_rotation,9);
	double *p_root_tan_origin =(double*) getPtrToField(p_root_tan, 0, "origin");
	
	float root_tan_T[3];
	// note that according to notes, T is the negative of the tan origin.
	for(int j =0;j<3; ++j) root_tan_T[j] = - p_root_tan_origin[j];
	double root_tan_scale = *(double*)getPtrToField(p_root_tan, 0, "swt"); //


    // tan responses
	float *p_scores=(float*)getPtrToField(p_tan,0,"score_map");
	int *p_deform = (int*)getPtrToField(p_tan,0,"arg_max_map");
	int *cluster_ind = (int*)getPtrToField(p_tan,0,"shape_ind");

    
	// get image labels, 1) projection matrix P 12 by n_image,
	// 2) view direction label 3 by n_image
	
	float *p_PMat = (float*) mxGetPr(prhs[i++]);
	float *p_viewMat = (float*)mxGetPr(prhs[i++]);
	

	// output path
	int n = (int)mxGetScalar(prhs[i++]);
	char *p_string =(char*) mxCalloc(n+1, sizeof(char));
	const char* p_out_path = const_cast<const char*> (p_string);
	mxGetString(prhs[i++],p_string,n+1);
 	

	// partition cluster
	double *p_if_plot = mxGetPr(prhs[i++]);

	shapeAndResponse calculator;
	calculator.initial(dims,h);
	calculator.my_projector.initialize(root_tan_scale,root_tan_rotation,root_tan_T,(int)dims[3],(int)dims[5],h);
	calculator.my_projector.updatePlane(target_plane.bu,target_plane.bv,target_plane.origin,
						target_plane.nt,target_plane.stp,target_plane.width,target_plane.height);
	calculator.generateShapeList();
	int n_shape = calculator.numShapes();

	// draw the shape in canonical view
	
	calculator.drawShapeInCanonicalView(cluster_ind,p_out_path);
	// draw shape in each image
	calculator.drawShapeInDeformedView(p_PMat, p_viewMat, p_sx, p_sy, p_zx, p_zy, cluster_ind, p_deform, p_out_path,p_if_plot);
     
	calculator.release();
    mxFree(p_M2);
	mxFree((void*)(const_cast<char*>(p_out_path)));
}
